#pragma once

#include "rclcpp/rclcpp.hpp"
#include <math.h>
#include <comm/canopen.h>

namespace ros_canopen_driver {

typedef struct CABLE_ENCODER_S
{
    uint8_t nodeId;
}CABLE_ENCODER_CFG;
//////////
typedef struct SERVOMOTOR_VMODE_CFG_S
{
    uint32_t acc;
    uint32_t dec;
}SERVOMOTOR_VMODE_CFG,*pSERVOMOTOR_VMODE_CFG;

typedef struct SERVOMOTOR_PMODE_CFG_S
{
    uint32_t vel;
    uint32_t acc;
    uint32_t dec;
}SERVOMOTOR_PMODE_CFG,*pSERVOMOTOR_PMODE_CFG;

typedef struct SERVOMOTOR_HMODE_CFG_S
{
    uint8_t home_mode;
    uint32_t velH;
    uint32_t velL;
    uint32_t acc;
    int homeOffset;
}SERVOMOTOR_HMODE_CFG,*pSERVOMOTOR_HMODE_CFG;

typedef struct SERVOMOTOR_CFG_S
{
    uint8_t nodeId;
    int8_t ctl_mode;
    bool rotateReverse;
    bool steerFeedbackReverse;

    bool homeNeed;
    double reduction_ratio;
    double encoder_resolution;
    SERVOMOTOR_VMODE_CFG vModeCfg;
    SERVOMOTOR_PMODE_CFG pModeCfg;
    SERVOMOTOR_HMODE_CFG hModeCfg;
    CABLE_ENCODER_CFG cableEncoderCfg;
}SERVOMOTOR_CFG,*pSERVOMOTOR_CFG;



typedef struct SERVOMOTOR_PORT_S
{
    SERVOMOTOR_CFG cfg;
}SERVOMOTOR_PORT,*pSERVOMOTOR_PORT_S;


struct ForkLiftParam {
    double lift_limit_down;          // fork lift limit down 
    double lift_limit_up;            // fork lift limit up 
    double lift_up_max_speed_limit;     // fork lift max speed limit
    double lift_down_max_speed_limit;     // fork lift max speed limit
    double lift_positon;             // for current posion
    bool is_lifting;                 // fork lift is_lifting signal
    bool cur_order;                  // current order
    int  cur_speed;                  // current speed
    // double lift_load_speed;          // fork lift up speed when load
    // double lift_unload_speed;        // fork lift down speed when unload
    // double lift_noload_speed;        // fork lift up/down speed when no load
    rclcpp::Time last_action_time;      // last action time
    rclcpp::TimerBase::SharedPtr lift_monitor;  // lift monintor timer
};

enum MotorState{
    Off,
    Init,
    Shutdown,
    Error,
    Halt,
    Recover,
    Ready
};

enum MotorCtlMode
{
    No_Mode = 0,
    Profiled_Position = 1,
    Profiled_Velocity = 3,
    Immediate_Velocity = -3,
    Homing = 6,
};

enum ServoType 
{ 
    NORMAL = 0, 
    STEER = 1, 
    LIFT = 2, 
    JACKING = 3, 
    ROTARY = 4,
    KEYA_A = 5,
    KEYA_B = 6,
    OTHERS = 7,
};


enum ServoManufacturer
{ 
    KINCO = 0, 
    ZAPI = 1, 
    CURTIS = 2, 
    NEWSTART = 3,
    YUFENG_CURTIS = 4,
    KEYA = 5,
};


// This is an abstract class. Only pure virtual function definitions allowed.
class Motor
{
public:
    
    virtual ~Motor() = 0;

    virtual int Motor_Init() = 0;

    virtual bool Motor_InitParam() = 0;

    virtual bool Motor_Enable(bool onFlag) = 0;

    virtual bool Motor_HomeMachine() = 0;

    virtual bool Motor_InitVmode() = 0;

    virtual bool Motor_InitPmode() = 0;

    virtual bool Motor_SwitchMode(int8_t mode) = 0;

 
    virtual void Motor_SetAcc(uint32_t acc) = 0;

    virtual void Motor_SetDec(uint32_t dec) = 0;

    virtual void Motor_SetTargetVelRad(double vel) = 0;

    virtual void Motor_SetTargetVelEnc(int vel) = 0;

    virtual void Motor_SetPmodeVel(uint32_t vel) = 0;

    virtual void Motor_SetTargetPosRad(double pos) = 0;


    virtual MotorState Motor_GetState() = 0;

    virtual double Motor_GetCurrentVel() = 0;

    virtual double Motor_GetCurrentPos() = 0;

    virtual double Motor_GetCurrent() = 0;

    virtual uint16_t Motor_GetStatusWord() = 0;

    virtual uint16_t Motor_GetErrCode() = 0;

    virtual int8_t Motor_GetMode() = 0;

    virtual std::string Motor_Err2userErr(const uint16_t& motorErr) = 0;


    virtual void Motor_Brake(bool on) = 0;

    virtual void Motor_Shutdown() = 0;

    virtual bool Motor_Halt() = 0;

    virtual bool Motor_Recover() = 0;

    virtual double Motor_GetTimestamp() = 0;

    std::shared_ptr<rclcpp::Node> node_;
    int             joint_index;
    int             nodeid;
    std::string     busname;
    std::string     joint_name;
    int             servo_manufacturer;
    int             servo_type;                 // servo type : 0 - normal wheel, 1 - steer 2 - lift, 3 - other(add later)
    double          ipeak;
    bool            use_extra_encoder;       // wheather use external encoder
    double          rad_to_meter;
    double          ratio;                      // servo ratios
    double          pos_scale;                  // postion scale 
    double          pos_offset;                 // postion offset
    double          ero_point_correction;       // steer wheel zero point correction, others allways 0 
    double          zero_point_correction;
};

}
